#include "./flash.h"
#include "../device/compat.h"

namespace driver::flash {

using namespace utils;

static void delay(volatile uint32_t x) {
	x *= 1000;
	while (x --);
} 

constexpr uint32_t FLASH_KEY1 = 0x45670123;
constexpr uint32_t FLASH_KEY2 = 0xCDEF89AB;

static void flash_unlock() {
    FLASH->KEYR = FLASH_KEY1;
    FLASH->KEYR = FLASH_KEY2;
}

static void flash_lock() {
    FLASH->CR |= 1 << 7;
}

enum class FlashError : uint8_t {
	OK = 0x00,
	BUY = 0x01,
	PARAMS_ERROR = 0x02,
	WRITE_PROTECTED = 0x03,
	TIME_OUT = 0x04,
};

static FlashError get_status() {
	uint32_t ret = FLASH->SR;
	if (ret & (0x01 << 0)) return FlashError::BUY;
	if (ret & (0x01 << 2)) return FlashError::PARAMS_ERROR;
	if (ret & (0x01 << 4)) return FlashError::WRITE_PROTECTED;
	return FlashError::OK;
}

static FlashError wait_complete(int32_t timeOut) {
	FlashError ret = FlashError::OK;
	do {
		ret = get_status();
		if (ret != FlashError::BUY) {
			break;
		}
		delay(1);
		timeOut --;
	} while (timeOut > 0);
	if (timeOut <= 0) { 
		return FlashError::TIME_OUT;
	}
	return ret;
}

static FlashError flash_write_16bit(uint32_t address, uint16_t data) {
	FlashError ret = wait_complete(0xFF);
	if (ret == FlashError::OK) {
		FLASH->CR |= 0x01;
		*((volatile uint16_t *)address) = data;
		ret = wait_complete(0xFF);
		if (ret != FlashError::BUY) {
			FLASH->CR &= ~0x01;
		}
	}
	if (ret == FlashError::OK) {
		if (data != *((volatile uint16_t *)address)) {
			return FlashError::PARAMS_ERROR;
		}
	}
	return ret;
}

Error write(uint32_t addr, const void *buf, uint32_t size) {
	FlashError ret = FlashError::OK;
	flash_unlock();
	auto p = reinterpret_cast<const uint16_t *>(buf);
	size = (size + 1) / 2;
	for (int32_t i = 0; i < size; i ++) {
		ret = flash_write_16bit(addr, p[i]);
		if (ret != FlashError::OK) {
			goto finish;
		}
		addr += 2;
	}
finish:
	flash_lock();
	return ((ret == FlashError::OK) ? ec::OK : ec::FLASH_WRITE_FAIL);
}

Error erase(uint32_t addr) {
	flash_unlock();
	FlashError ret = wait_complete(1000);
	if (ret == FlashError::OK) {
		FLASH->CR |= 1 << 1;
		FLASH->AR = addr;
		FLASH->CR |= 1 << 6;
		ret = wait_complete(1000);
		if (ret != FlashError::BUY) {
			FLASH->CR &= ~(1 << 1);
		}
	}
	flash_lock();
	return ((ret == FlashError::OK) ? ec::OK : ec::FLASH_ERASE_FAIL);
}

Error erase(uint32_t addr, uint32_t size) {
	uint32_t index = 0;
	Error err;
	for (;index < size;) {
		err = erase(addr + index);
		if (err) {
			goto finish;
		}
		index += SECTOR_SIZE;
	}
finish:
	return err;
}

Error move(uint32_t target, uint32_t src, uint32_t size) {
	Error err = erase(target, size);
	if (err) {
		return err;
	}
	return write(target, reinterpret_cast<const void *>(src), size);
}

void read(uint32_t addr, void *buf, uint32_t size) {
	auto p = static_cast<uint16_t *>(buf);
	auto a = reinterpret_cast<const uint16_t *>(addr);
	size /= 2;
	for (int32_t i = 0; i < size; i ++) {
		p[i] = a[i]; 
	}
}

}